#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Ultrasonic,     sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     LeftMotor,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightMotor,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     MiddleMotor,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  motor[MiddleMotor]=0;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  motor[MiddleMotor] = 0;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  motor[MiddleMotor] = 0;
  wait1Msec(duration);
}


void Drift (int power, int duration)
{
	motor[LeftMotor]= 0;
	motor[RightMotor]= 0;
	motor[MiddleMotor]= power;
	wait1Msec(duration);
}

bool myFlag = false;

task main()
{
	while(SensorValue[Ultrasonic]>50)
	{
		Forward(50,50);
	}

	if(SensorValue[IR1200]==5)
	{
		myFlag=true;
	}

	else if(SensorValue[IR1200]!=5)
	{
		Backward(50,1500);
		Drift(-70,2100);
		while(SensorValue[Ultrasonic]>65)
		{
			Forward(50,50);
		}

		if(SensorValue[IR1200]==5)
		{
			myFlag=true;
		}

		else if(SensorValue[IR1200]!=5)
		{
			Backward(50,2000);
			Drift(70,1050);
			Forward(0,1000);
			Forward(100,3000);
			myFlag=true;
		}
	}
}
